#pragma ident "$Id: SolutionStoreForPCE.cpp $"

/**
 * @file SolutionStoreForPCE.cpp
 * Store solution for kalman filter.
 */


#include "SolutionStoreForPCE.hpp"


using namespace std;


namespace gpstk
{

    /** Prepare state vector, covariance matrix and index.
     *
     * @return this object.
     */
    SolutionStoreForPCE& SolutionStoreForPCE::Prepare(const sourceRinex& gData)
    {

        SourceIDSet newSourceSet = gData.getSourceIDSet();
        SatIDSet newSatSet = gData.getSatIDSet();

        int newNumUnknown(newSourceSet.size()*2 + newSatSet.size()*2);

        for(sourceDataMap::const_iterator sdmIt = gData.body.begin();
            sdmIt != gData.body.end();
            ++sdmIt)
        {
            satTypeValueMap stvm( sdmIt->second );
            for(satTypeValueMap::const_iterator stvmIt = stvm.begin();
                stvmIt != stvm.end();
                ++stvmIt)
            {
                typeValueMap tvm( stvmIt->second );

                if(tvm.find(TypeID::prefitLForPCE) != tvm.end())
                    newNumUnknown++;
            }
        }

        numUnknown = newNumUnknown;

        Vector<double> newState(newNumUnknown,0.0);
        Matrix<double> newCovar(newNumUnknown,newNumUnknown,0.0);

        SourceIntMap newSourceIndex;
        SatIntMap newSatIndex;
        SourceSatIntMap newAmbiIndex;

        int oldID(0), newID(0);

        // source-related parameters, clock offset and zwd
        for(SourceIDSet::iterator itSource = newSourceSet.begin();
            itSource != newSourceSet.end();
            ++itSource)
        {
            SourceID source(*itSource);

            newSourceIndex[source] = newID;

            if(sourceIndex.find(source) != sourceIndex.end())
            {
                oldID = sourceIndex[source];

                for(int i=0; i<2; i++)
                {
                    newState(newID+i) = state(oldID+i);

                    for(int j=0; j<2; j++)
                    {
                        newCovar(newID+i,newID+j) = covar(oldID+i,oldID+j);
                    }
                }
            }
            else
            {
                newState(newID+0) = 0.0;
                newState(newID+1) = 0.0;
                newCovar(newID+0,newID+0) = 1e+2*1e+2;
                newCovar(newID+1,newID+1) = 0.5*0.5;
            }

            newID = newID + 2;

        } // End of 'for(SourceIDSet::iterator...)'


        // sat-related parameters, clock offset
        for(SatIDSet::iterator itSat = newSatSet.begin();
            itSat != newSatSet.end();
            ++itSat)
        {
            SatID sat(*itSat);

            newSatIndex[sat] = newID;

            double clock( satClock[sat] );

            if( satIndex.find(sat) != satIndex.end() )
            {
                oldID = satIndex[sat];

                for(int i=0; i<1; i++)
                {
                    newState(newID+i) = state(oldID+i);

                    for(int j=0; j<1; j++)
                    {
                        newCovar(newID+i,newID+j) = covar(oldID+i,oldID+j);
                    }
                }
            }
            else
            {
                newState(newID+0) = clock;
                newCovar(newID+0,newID+0) = 1e+2*1e+2;
            }

            newID = newID + 2;

        } // End of 'for(SatIDSet::iterator...)'


        for(sourceDataMap::const_iterator sdmIt = gData.body.begin();
            sdmIt != gData.body.end();
            ++sdmIt)
        {
            SourceID source( sdmIt->first );

            SatIntMap satIndexTemp1;
            SatIntMap satIndexTemp2;

            satTypeValueMap stvm( sdmIt->second );
            for(satTypeValueMap::const_iterator stvmIt = stvm.begin();
                stvmIt != stvm.end();
                ++stvmIt)
            {
                SatID sat( stvmIt->first );

                typeValueMap tvm( stvmIt->second );

                if(tvm.find(TypeID::prefitLForPCE) == tvm.end()) continue;

                satIndexTemp2[sat] = newID;

                // old source
                if( ambiIndex.find(source) != ambiIndex.end() )
                {
                    satIndexTemp1 = ambiIndex[source];

                    // old sat of old source
                    if( satIndexTemp1.find(sat) != satIndexTemp1.end() )
                    {
                        oldID = satIndexTemp1[sat];

                        // cycle slip: yes
                        if( tvm[TypeID::CSL1] > 0.0 )
                        {
                            newState(newID+0) = 0.0;
                            newCovar(newID+0,newID+0) = 1e+2*1e+2;
                        }
                        // cycle slip: no
                        else
                        {
                            newState(newID+0) = state(oldID+0);
                            newCovar(newID+0,newID+0) = covar(oldID+0,oldID+0);
                        }
                    }
                    // new sat of old source
                    else
                    {
                        newState(newID+0) = 0.0;
                        newCovar(newID+0,newID+0) = 1e+2*1e+2;
                    }
                }
                // new source
                else
                {
                    newState(newID+0) = 0.0;
                    newCovar(newID+0,newID+0) = 1e+2*1e+2;
                }

                newID = newID + 1;
            }

            newAmbiIndex[source] = satIndexTemp2;
        }

        state = newState;
        covar = newCovar;

        sourceIndex = newSourceIndex;
        satIndex = newSatIndex;
        ambiIndex = newAmbiIndex;

        return (*this);

    } // End of method 'SolutionStoreForPCE::Prepare(...)'

}  // End of namespace gpstk
